#include "gps_filter.h"


GPS_Filter::GPS_Filter()
{
    longitudeFilter = new kalmanFilter(0);
    latitudeFilter = new kalmanFilter(0);
}

GPS_Filter::GPS_Filter(float lon, float lat, float R)
{
    longitudeFilter = new kalmanFilter(lon, R);
    latitudeFilter = new kalmanFilter(lat, R);
}

GPS_Filter::GPS_Filter(float lon, float lat, float R, float qd)
{
    longitudeFilter = new kalmanFilter(lon, R, qd);
    latitudeFilter = new kalmanFilter(lat, R, qd);
}


GPS_Filter::~GPS_Filter()
{
    delete longitudeFilter;
    delete latitudeFilter;
}

GPS_DATA GPS_Filter::GPS_Filt(float lon, float lat, float t)
{
    GPS_DATA result;
    result.longitude = longitudeFilter->kalmanFilt(lon, t);
    result.latitude = latitudeFilter->kalmanFilt(lat, t);
    return result;
}

GPS_DATA GPS_Filter::GPS_Filt(float lon, float lat, float t, float R)
{
    GPS_DATA result;
    result.longitude = longitudeFilter->kalmanFilt(lon, t, 2000 * R);
    result.latitude = latitudeFilter->kalmanFilt(lat, t, 2000 * R);
    return result;
}
